/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "BetBANNLocalizer.h"
/**
 * Constructor for line map input
 * @param *mapfile path to line map file
 * @param resolution The requested resolution for occupancy grid
 */
CBetBANNLocalizer::CBetBANNLocalizer(const char *mapfile,float resolution){
		cellAlignmentData = NULL;
		grid = NULL;
		distanceTransform = NULL;
		fprintf(stderr,"CBetBANNLocalizer: Initializing Occupancy Grid\n");
		grid = new TOccupancyGrid(1, 1, resolution, Grid::TCoord2D(0,0));
		grid->loadLineMap(mapfile, 1.0,1.0);
		map_resolution = resolution;
		paramIsMapinit = true;
		initialize();
}

/**
 * Constructor for occu map input
 * @param *mapfile path to line map file
 */
CBetBANNLocalizer::CBetBANNLocalizer(const char *mapfile){
		cellAlignmentData = NULL;
		grid = NULL;
		distanceTransform = NULL;
		fprintf(stderr,"CBetBANNLocalizer: Initializing Occupancy Grid\n");	
		grid = new TOccupancyGrid();
		grid->load(mapfile);
		map_resolution = grid->getResolution();
		paramIsMapinit = true;
		initialize();
}

CBetBANNLocalizer::~CBetBANNLocalizer(){
		if(grid) delete grid;
		if(distanceTransform) free(distanceTransform);				
		if(cellAlignmentData) free(cellAlignmentData);
		if(pb_ind)free(pb_ind);
		if(pb_cost)free(pb_cost);
}


/**
 * Constructor for empty map = SLAM version
 * @param size the size of the map in meters
 * @param resolution The requested resolution for occupancy grid
 */
CBetBANNLocalizer::CBetBANNLocalizer(float map_size,float resolution){
		cellAlignmentData = NULL;
		grid = NULL;
		distanceTransform = NULL;
		fprintf(stderr,"CBetBANNLocalizer: Initializing Occupancy Grid\n");
		grid = new TOccupancyGrid(map_size, map_size, resolution, Grid::TCoord2D(0,0));
		map_resolution = resolution;
		paramIsMapinit = false;
		grid->setGridToValue(0.5);
		initialize();
}

/**
* Initialize ANN related stuff
**/
void CBetBANNLocalizer::intializeANN(){
		int				k				= 1;			// number of nearest neighbors
		int				dim			= 2;			// dimension
		eps			= 0.03;			// error bound
		maxPts	= 100000;			// maximum number of data points (more will segfault!!)
		
		queryPt = annAllocPt(dim);					// allocate query point
		dataPts = annAllocPts(maxPts, dim);			// allocate data points (THE MAP)
		nnIdx = new ANNidx[k];						// allocate near neigh indices
		dists = new ANNdist[k];						// allocate near neighbor dists
		kdTree = NULL;
}


/**
 * Update the environment measurement to the map
 * Transforms the data to reference frame and recomputes the distance transform
 * @param &meas [in] reference to CEnvironmentMeasurement2D class 
 **/
void CBetBANNLocalizer::setMeasurementToMap(CEnvironmentMeasurement2D &meas){
		float r,phii;
		TMeasurementPoint coord;
		paramIsMapinit = true;
		for(unsigned int i=0;i<meas.points.size();i++){
				r = meas.points[i].getDist();
				phii = meas.points[i].getAngle();
				coord.x = r*cos(phii+meas.frame.a_ref)+meas.frame.x_ref;
				coord.y = r*sin(phii+meas.frame.a_ref)+meas.frame.y_ref;
				GlobalMap.addPoint(coord);
		}
}
void CBetBANNLocalizer::setMeasurementToMap(Grid::TScan &meas, float x,float y, float a, float P, bool updateDist,float max_laser_dist){
		setMeasurementToMap(meas, x,y, a);
}
/**
 * Update the environment measurement to the map
 * Transforms the data to reference frame and recomputes the distance transform
 * @param &meas [in] reference to Grid::TScan 
 * @param x,y,a pose where the scan was taken 
 **/
void CBetBANNLocalizer::setMeasurementToMap(Grid::TScan &meas, float x,float y, float a){
		paramIsMapinit = true;
		TMeasurementPoint coord;
		for(int i=0;i<meas.N;i++){
				if(meas.r[i]>0){
						coord.x = x + meas.r[i]*cos(meas.a[i]+a); 
						coord.y = y + meas.r[i]*sin(meas.a[i]+a);
						GlobalMap.addPoint(coord); 
				}
		}
		
		/*
		if(max_laser_dist>0){
				grid->setScanFillProb(x,y,a, meas,(float)(1.0-P+0.2),P,max_laser_dist);
		}else{				
				grid->setScanFillProb(x,y,a, meas,(float)(1.0-P+0.2),P);
		}*/
		
}
void CBetBANNLocalizer::updateDistTransform(){
		paramIsMapinit = true;
		maxdist = grid->distanceTransform(distanceTransform, 0.8); ///< Compute distance transform
}

/**
 * Localizator interface
 * @param &meas [in] reference to CEnvironmentMeasurement2D class 
 * @param &out [out] a reference to CMatchStorage, which is filled according to results
 * @return true, if all was ok.
 */
bool CBetBANNLocalizer::localize(CEnvironmentMeasurement2D &meas, CMatchStorage &out){
		RegistrationPntData   pts;
		TwoDPoint *pt; //[181]; ///< measurement points in (x,y)
		float *r;      //< measurement distances
		CellAlignment zeroAlignment;               ///< initial pose for first cell
		CellAlignmentSpan initialSearchSpaceSpan;  ///< The size of the first cell (search area)
		
		if(!paramIsMapinit){
				setMeasurementToMap(meas);
				return false;	
		}
		
		if(kdTree != NULL) delete kdTree; ///Fixme - maybe not allways??
		ANNpoint			p;				// query point
		///Set data
		if(GlobalMap.size() > maxPts) fprintf(stderr,"CBetBANNLocalizer::localize::Expect Crash!! \n");
		for(int i=0;i<GlobalMap.size();i++){
				p=dataPts[i];
				p[0] = GlobalMap.points[i].x;
				p[1] = GlobalMap.points[i].y;
		}
		
		kdTree = new ANNkd_tree(// build search structure
														dataPts,					// the data points
														GlobalMap.size(),		// number of points
														2);						// dimension of space
		
		
		initialSearchSpaceSpan.dx = meas.frame.x_err;
		initialSearchSpaceSpan.dy = meas.frame.y_err;
		initialSearchSpaceSpan.dphii = meas.frame.a_err;
		zeroAlignment.x = meas.frame.x_ref;
		zeroAlignment.y = meas.frame.y_ref;
		zeroAlignment.phii = meas.frame.a_ref;
		
		cellAlignmentData[0].cellAlignCentre=zeroAlignment;
		cellAlignmentData[0].cellAlignHalfSpan=initialSearchSpaceSpan;
		
		pt = (TwoDPoint *) malloc(meas.size()*sizeof(TwoDPoint));
		
		r = (float *) malloc(meas.size()*sizeof(float));
		/**
		* Copy data
		*/
		for(unsigned int i=0;i<meas.size();i++){
				pt[i].x = meas.points[i].x; 
				pt[i].y = meas.points[i].y;
				r[i] = sqrt(pt[i].x*pt[i].x+pt[i].y*pt[i].y);
		}
		pts.dataPnts=pt; 
		pts.nbOfRegDataPnts= meas.size();
		
		//fprintf(stderr,"Registering data... please hold...\n");
		int num = viewPointRegistration(cellAlignmentData,grid,distanceTransform,&pts,r); //,paramTh,paramFdist);
		//fprintf(stderr,"Registration completed. Now checking all %d nodes... please hold\n",num);
		
		out.storage.clear(); ///< clear first
		
		TPoseMatch pm;
		for(unsigned int i=0;i<num;i++){
				if(out.isBetter(cellAlignmentData[i].cellCenterScore)){ ///cellAlignmentData[i].upperBoundForCellScore
						pm.setPose(cellAlignmentData[i].cellAlignCentre.x, 
											 cellAlignmentData[i].cellAlignCentre.y,
											 cellAlignmentData[i].cellAlignCentre.phii);
						pm.setArea(cellAlignmentData[i].cellAlignHalfSpan.dx,
											 cellAlignmentData[i].cellAlignHalfSpan.dy,
											 cellAlignmentData[i].cellAlignHalfSpan.dphii);
						pm.weight = cellAlignmentData[i].cellCenterScore;
						out.update(pm);
				}	
		}
		out.sort();
		
		///register now
		free(r);
		free(pt);
		delete kdTree;
		kdTree = NULL;
		return true;
}


/**
* Sets the tuning parameters for algorithm
* @param fdist big number will cause more angles to be viewed
* @param th small number will cause more accurate search
**/
void CBetBANNLocalizer::tuneParameters(float fdist_,float th){
		paramTh = th;
		paramFdist = fdist_;
} 

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/// Private methods
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/**
 * Internal initializer
 */
bool CBetBANNLocalizer::initialize(){
		grid->getDim(Nx,Ny);
		distanceTransform = (float *) malloc(Nx*Ny*sizeof(float));
		if(distanceTransform==NULL){
				fprintf(stderr,"ERROR:Unable to reserve memory for the Distance Transform\n");
				exit(1);
		}
		fprintf(stderr,"CBetBANNLocalizer:Initializing distance transform\n");
		
		maxdist = grid->distanceTransform(distanceTransform, 0.9); ///< Compute distance transform
		
		fprintf(stderr,"CBetBANNLocalizer:Initializing Cell Aligment Data\n");
		cellAlignmentData = (CellAlignmentData *)calloc( MAXNBOFDATAPNTCELLS, sizeof( CellAlignmentData ) );
		
		if(cellAlignmentData == NULL){
				fprintf(stderr,"ERROR: Unable to reserve memory for the Cell Aligment Data!\n");
				exit(1);
		}
		
		if (( pb_ind=(unsigned int*)malloc(MAXNBOFDATAPNTCELLS*sizeof(int))) == NULL) printf("malloc error");
		if (( pb_cost=(float*)malloc(MAXNBOFDATAPNTCELLS*sizeof(float))) == NULL) printf("malloc error");
		
		paramTh=0.6;
		paramFdist=4.0;
		//paramMinSpan=map_resolution;
		//paramISpan=1.0/map_resolution;
		paramMinReguiredRegDataPntNb=5;
		paramMinSpan=0.15;
		paramISpan=1.0/paramMinSpan;
		
		intializeANN();
		
		fprintf(stderr,"CBetBANNLocalizer:Initialization done\n");
		return true;
}

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////

int	CBetBANNLocalizer::computeShortestDistanceBtwLocalMapElementWrtElementsInGlobalMap(
				TwoDPoint				pntInTheLocalMap,
											float					cosPhii,
											float					sinPhii,
											float*	globalMap,
											TOccupancyGrid *O,
											float					deltaC,
											TwoDPoint			currentAlignment,
											float*					distance,
											float*					best,
											float 					pTh)
{
		TwoDPoint		transfPnt;
		float			shortestDistance;
		float ssdmin,dist,di;
		int x,y;

		/* Computed the coordinates of the transformed local map point */
		transfPnt.x = currentAlignment.x + pntInTheLocalMap.x*cosPhii - pntInTheLocalMap.y*sinPhii;
		transfPnt.y = currentAlignment.y + pntInTheLocalMap.x*sinPhii + pntInTheLocalMap.y*cosPhii;
	
	

		/* Find the shortest distance of the transformed point w.r.t the points in the global map */
				
  //     dist=getDist(globalMap, O, transfPnt.x,transfPnt.y);
		queryPt[0] = transfPnt.x;
		queryPt[1] = transfPnt.y;
		kdTree->annkSearch(	// search
												queryPt,	// query point
												1,				// number of near neighbors
												nnIdx,		// nearest neighbors (returned)
												dists,		// distance (returned)
												eps);	
		dist = sqrt(dists[0]);
		
		//fprintf(stderr,"tf:(%.2f %.2f) dist=%.4f \n",transfPnt.x,transfPnt.y,dist);
						
		shortestDistance=dist*dist;
	
		di=(dist-deltaC);
		if(di<0)
				di=0;
		ssdmin=di*di;
	
		if(shortestDistance>pTh)
				shortestDistance=pTh;
		
		*distance = shortestDistance;
	
		if(ssdmin>pTh)
				ssdmin=pTh;
	
		*best=ssdmin;

		return(0);
}


/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////

int	CBetBANNLocalizer::computeDistanceToBeAddedToComputeUpperBoundForASingleElementScore(
				TwoDPoint			pntInTheLocalMap,
											float				deltaXYZC,
											float				sinPhiiPer2,
											float				cosPhii,
											float				sinPhii,
											CellAlignmentSpan	cellAlignHalfSpan,
											float*				deltaC )
{	
		float	distance;
		float	deltaPhiiC;



		distance = sqrt(pntInTheLocalMap.x*pntInTheLocalMap.x + 
						pntInTheLocalMap.y*pntInTheLocalMap.y);



		deltaPhiiC = fabs(2.*distance*sinPhiiPer2);






/* 3.1.01 */ 	*deltaC = (deltaXYZC + deltaPhiiC); 



							return(0);
}

/////////////////////////////////////////////////////////////////////////////////

int	CBetBANNLocalizer::calc_best_worst( 
				RegistrationPntData*	localMap,
							float*	r,
							float*	globalMap,
							TOccupancyGrid *O,
							CellAlignmentData*		cellAlignmentData,
							float*					upperBoundAvgLikelihood,
							float*					Likelihood,
							float					pTh )
{
		float	cosPhii;
		float	sinPhii;
		float	deltaSinPhiiPer2;
		float	deltaXYZC;
		float	deltaC;
		float	distance;
		float	accumulatedLikelihoodb;
		float	accumulatedLikelihoodw;
		float span,span2;
		///static struct timespec start, stop;
		int		i;
		TwoDPoint center;

		float best; //,runtime;

//	clock_gettime(CLOCK_REALTIME,&start);
		cosPhii = cos((cellAlignmentData->cellAlignCentre).phii); //*paramISpan;
		sinPhii = sin((cellAlignmentData->cellAlignCentre).phii); //*paramISpan;
		deltaSinPhiiPer2 = 2.*fabs(sin((cellAlignmentData->cellAlignHalfSpan).dphii/2.0));
		deltaXYZC = sqrt(	(cellAlignmentData->cellAlignHalfSpan).dx*(cellAlignmentData->cellAlignHalfSpan).dx +
						(cellAlignmentData->cellAlignHalfSpan).dy*(cellAlignmentData->cellAlignHalfSpan).dy);
	

		span=pTh+paramMinSpan;
		span2=span*span;
		/* Reset the upper bound likelihood values */
		*upperBoundAvgLikelihood = 0.0;
		center.x=cellAlignmentData->cellAlignCentre.x; //*paramISpan+Nx/2.;
		center.y=cellAlignmentData->cellAlignCentre.y; //*paramISpan-Ny/2.;
		if((localMap->nbOfRegDataPnts > paramMinReguiredRegDataPntNb))
		{
				/* 8.6.01, Compute upper bound by means of cone map data */
				accumulatedLikelihoodb = 0.0;
				accumulatedLikelihoodw = 0.0;
				for(i=0; i<localMap->nbOfRegDataPnts; i++)
				{

						deltaC = deltaXYZC+r[i]*deltaSinPhiiPer2;
			
						computeShortestDistanceBtwLocalMapElementWrtElementsInGlobalMap(localMap->dataPnts[i], cosPhii, sinPhii,globalMap,O,deltaC,
										center, &distance,&best,span2);
			
		
						accumulatedLikelihoodw += distance;
						accumulatedLikelihoodb += best;
				}
		
				*upperBoundAvgLikelihood = accumulatedLikelihoodb/(localMap->nbOfRegDataPnts);
				(*Likelihood) = accumulatedLikelihoodw/(localMap->nbOfRegDataPnts);
		}

  //	clock_gettime(CLOCK_REALTIME,&stop);	
//	runtime = (stop.tv_sec - start.tv_sec)*1000 + (float)(stop.tv_nsec - start.tv_nsec) / 1000000.0;
	
		cellAlignmentData->upperBoundForCellScore = *upperBoundAvgLikelihood;
		cellAlignmentData->cellCenterScore = (*Likelihood);
	
		return(0);
}

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////


CellAlignmentData*		CBetBANNLocalizer::divideEachCellInTheCurrentLevelIntoTwoSubCells( 
				CellAlignmentData*	cellAlignmentData,
											unsigned int index,
											unsigned int		*indexLess,
											unsigned int		*indexMore,
											unsigned int		*nbOfCells,
											float	filteredDataPntDistanceWithinTheLocalMap,
											float	*maxSpan)
{

		float	newXLess=0;
		float	newXMore=0;
		float	newYLess=0;
		float	newYMore=0;
		float	newPhiiLess=0;
		float	newPhiiMore=0;

		float	newXHalfSpan=0;
		float	newYHalfSpan=0;
		float	newPhiiHalfSpan=0;

		float	phiiHalfSpanEqualInMeters=0,span=0;

		CellAlignment		alignmentCentre;
		CellAlignmentSpan	alignmentSpan;


		/* Compute a value in [meters] for the heading angle half span */
		phiiHalfSpanEqualInMeters = 
						fabs(2*filteredDataPntDistanceWithinTheLocalMap*sin(cellAlignmentData[index].cellAlignHalfSpan.dphii/2.0));

		/* Check which one of the original translational half spans is the largest */
		if((cellAlignmentData[index].cellAlignHalfSpan.dx >= cellAlignmentData[index].cellAlignHalfSpan.dy) &&
					(cellAlignmentData[index].cellAlignHalfSpan.dx >= phiiHalfSpanEqualInMeters))
		{
				newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx/2.0;
				newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy;
				newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii; 

				newXLess = cellAlignmentData[index].cellAlignCentre.x - newXHalfSpan;
				newXMore = cellAlignmentData[index].cellAlignCentre.x + newXHalfSpan;
				newYLess = cellAlignmentData[index].cellAlignCentre.y;
				newYMore = cellAlignmentData[index].cellAlignCentre.y;
				newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii;
				newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii;
				span=cellAlignmentData[index].cellAlignHalfSpan.dx;
		}
		else if((cellAlignmentData[index].cellAlignHalfSpan.dy >= cellAlignmentData[index].cellAlignHalfSpan.dx) &&
							 (cellAlignmentData[index].cellAlignHalfSpan.dy >= phiiHalfSpanEqualInMeters))
		{
				newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx;
				newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy/2.0;
				newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii; 

				newXLess = cellAlignmentData[index].cellAlignCentre.x;
				newXMore = cellAlignmentData[index].cellAlignCentre.x;
				newYLess = cellAlignmentData[index].cellAlignCentre.y - newYHalfSpan;
				newYMore = cellAlignmentData[index].cellAlignCentre.y + newYHalfSpan;
				newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii;
				newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii;
				span=cellAlignmentData[index].cellAlignHalfSpan.dy;
		}
		else if((phiiHalfSpanEqualInMeters >= cellAlignmentData[index].cellAlignHalfSpan.dx) &&
							 (phiiHalfSpanEqualInMeters >= cellAlignmentData[index].cellAlignHalfSpan.dy) )
		{
				newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx;
				newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy;
				newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii/2.0; 

				newXLess = cellAlignmentData[index].cellAlignCentre.x;
				newXMore = cellAlignmentData[index].cellAlignCentre.x;
				newYLess = cellAlignmentData[index].cellAlignCentre.y;
				newYMore = cellAlignmentData[index].cellAlignCentre.y;
				newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii - newPhiiHalfSpan;
				newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii + newPhiiHalfSpan;
				span=phiiHalfSpanEqualInMeters;
		}

		(*maxSpan)=span;
	
		alignmentSpan.dx = newXHalfSpan;
		alignmentSpan.dy = newYHalfSpan;
		alignmentSpan.dphii = newPhiiHalfSpan; //21.12.00
		/* Now, create and add new cells into the global array */
		/* First */
		alignmentCentre.x = newXLess;
		alignmentCentre.y = newYLess;
		alignmentCentre.phii = newPhiiLess; //21.12.00
		(*indexLess)=(*nbOfCells);
		cellAlignmentData[(*indexLess)].cellAlignCentre=alignmentCentre;
		cellAlignmentData[(*indexLess)].cellAlignHalfSpan=alignmentSpan;
		(*nbOfCells)++;


		/* Last */
		alignmentCentre.x = newXMore;
		alignmentCentre.y = newYMore;
		alignmentCentre.phii = newPhiiMore; //21.12.00
		(*indexMore)=(*nbOfCells);
		cellAlignmentData[(*indexMore)].cellAlignCentre=alignmentCentre;
		cellAlignmentData[(*indexMore)].cellAlignHalfSpan=alignmentSpan;
		(*nbOfCells)++;

		return(0);
}
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////

/*min-heap remove and return minimum value node */
void CBetBANNLocalizer::up_heap(float *heap,unsigned int *indi, float val, unsigned int ind,unsigned int *N)
{
		unsigned int parent,child,tmpi;
		float tmp;

		heap[*N]=val;
		indi[*N]=ind;

		child=*N;
		(*N)++;
		if(*N==1)
				return;
		for(;;)
		{
				parent=(child-1)>>1;
				if(heap[parent]>heap[child])
				{
						tmp=heap[parent];
						heap[parent]=heap[child];
						heap[child]=tmp;
						tmpi=indi[parent];
						indi[parent]=indi[child];
						indi[child]=tmpi;

						if(parent==0)
								break;
				}
				else
						break;
				child=parent;
		}
}

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////

void CBetBANNLocalizer::down_heap(float *heap,unsigned int *indi, float *val,unsigned  int *ind, unsigned int *N)
{
		unsigned int parent,child1,child2,tmpi;
		float tmp;

		(*N)--;
		*val=heap[0];
		*ind=indi[0];
		heap[0]=heap[*N];
		indi[0]=indi[*N];

		parent=0;
		child1=(parent<<1)+1;
		child2=(parent<<1)+2;
		for(;;)
		{
				if(child1>(*N))
						break;
				if(child2>(*N))
						child2=child1;

				if(heap[child1]<heap[child2])
				{
						if(heap[child1]<heap[parent])
						{
								tmp=heap[parent];
								heap[parent]=heap[child1];
								heap[child1]=tmp;
								tmpi=indi[parent];
								indi[parent]=indi[child1];
								indi[child1]=tmpi;

								parent=child1;
								child1=(parent<<1)+1;
								child2=(parent<<1)+2;
						}
						else
								break;
				}
				else
				{ 
						if(heap[child2]<heap[parent])
						{
								tmp=heap[parent];
								heap[parent]=heap[child2];
								heap[child2]=tmp;
								tmpi=indi[parent];
								indi[parent]=indi[child2];
								indi[child2]=tmpi;
								parent=child2;
								child1=(parent<<1)+1;
								child2=(parent<<1)+2;
						}
						else
								break;
				}
		}
}

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////


int  CBetBANNLocalizer::BetBloc(unsigned int *pb_ind,
								float *pb_cost,
								CellAlignmentData* 	cellAlignmentData,
								TOccupancyGrid *O,
								float*	globalMap,
								RegistrationPntData*	localMap,
								float *r)
								//float filteredDataPntDistanceWithinTheLocalMap
								//float paramTh
								//)
{
		unsigned int j,ind1,ind2;
		unsigned int index,a,cnt=0,numnode=1;
		float best[2],worst[2],minworst,maxworst,val,max_span;
		float filteredDataPntDistanceWithinTheLocalMap = paramFdist;
		j=0;
 
		calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[0],&best[0], &worst[0],paramTh );

		minworst=worst[0];
		maxworst=best[0];
		//printf("minworts:%f\n",minworst);
		//printf("maxworts:%f\n",maxworst);
		a=0;
		up_heap(pb_cost,pb_ind,best[0],0, &j);

		down_heap(pb_cost,pb_ind,&val,&index,&j);
		while(val<=minworst)
		{
				/* 1. */

				divideEachCellInTheCurrentLevelIntoTwoSubCells( cellAlignmentData,index, &ind1,&ind2,&numnode,filteredDataPntDistanceWithinTheLocalMap,&max_span );
	
				calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[ind1],&best[0], &worst[0],paramTh );
				calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[ind2],&best[1], &worst[1],paramTh );
     
    
				if(best[0]<val)
						best[0]=val;
				if(best[1]<val)
						best[1]=val;
	
				if(max_span>paramMinSpan)
						if(best[0]<minworst)
								up_heap(pb_cost,pb_ind,best[0],ind1, &j);
	
				if(worst[0]<minworst) /* 2. */
				{    
						minworst=worst[0];
						a=ind1; 
				}	    
				if(max_span>paramMinSpan)    
						if(best[1]<minworst)
								up_heap(pb_cost,pb_ind,best[1],ind2, &j);
	    
				if(worst[1]<minworst) /* 2. */
				{    
						minworst=worst[1];
						a=ind2;
				}
				if(minworst<val) printf("error val: %f minworst: %f index :%d ind1: %d ind2: %d \n",val,minworst,index,ind1,ind2);
				cnt++;
				if((cnt%100000)==0)
						printf("nodenum:%d maxspan:%f b:%f val:%f w:%f mw:%f\n",numnode,max_span,best[0],val,worst[0],minworst);
	
				/* 3. */
				if(j==0)
						break;
				if(numnode>MAXNBOFDATAPNTCELLS-1)
						break;
				down_heap(pb_cost,pb_ind,&val,&index,&j);
	
		}
#if 0			
		printf("a:%d, ind1:%d,ind2:%d\n",(int)a,(int)ind1,(int)ind2);
		printf("index:%d\n",int(a));
		printf("span:%f\n",max_span);
		printf("minworts:%f\n",minworst); ///Score (pienempi parempi)
		printf("maxworts:%f\n",maxworst);
		printf("x:%f y:%f phii:%f\n",cellAlignmentData[a].cellAlignCentre.x,cellAlignmentData[a].cellAlignCentre.y,cellAlignmentData[a].cellAlignCentre.phii);
		printf("best0:%f\n",best[0]);
		printf("best1:%f\n",best[1]);
		printf("numnod:%d\n",numnode);
	
		//xx  = cellAlignmentData[a].cellAlignCentre.x;
		//yy  = cellAlignmentData[a].cellAlignCentre.y;
		//aa  = cellAlignmentData[a].cellAlignCentre.phii;
		//nn[0]=(float)a;
		//nn[1]=max_span;
		//nn[2]=minworst;
		if(0)
		{
				minworst=cellAlignmentData[0].cellCenterScore;
				maxworst=cellAlignmentData[0].cellCenterScore;
				val=cellAlignmentData[0].cellCenterScore;
				for(i=0;i<numnode;i++)
				{
 	//if(minworst<cellAlignmentData[i].cellCenterScore minworst=cellAlignmentData[i].cellCenterScore;
	//if(maxworst>cellAlignmentData[i].cellCenterScore) maxworst=cellAlignmentData[i].cellCenterScore;
    //	val+=cellAlignmentData[i].cellCenterScore;
						printf("node:%f %f %f %f %f %f\n",cellAlignmentData[i].cellAlignCentre.x,cellAlignmentData[i].cellAlignCentre.y,cellAlignmentData[i].cellAlignCentre.phii,
									 cellAlignmentData[i].upperBoundForCellScore,cellAlignmentData[i].cellAlignHalfSpan.dx,cellAlignmentData[i].cellCenterScore);
				}
				for(i=0;i<Nx;i++)
						for(j=0;j<Ny;j++)
								printf("map: %d %d %f\n",i,j,globalMap[i*Ny+j]);
		}
		printf("mintime:%f\n",minworst);
		printf("maxtime:%f\n",maxworst);
		printf("meantime:%f\n",val/numnode);
#endif    
		return numnode;
}


/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/// This does THE job
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
int	CBetBANNLocalizer::viewPointRegistration(CellAlignmentData *cellAlignmentData, 
																						TOccupancyGrid *O,
																						float *globalMap,
																						RegistrationPntData *localMap,
																						float *r) 
																						//float paramTh, 
																						//float paramFdist)
{

		
		int numOfNodes;
		numOfNodes = BetBloc(pb_ind,pb_cost,cellAlignmentData,O,globalMap,localMap,r); //,paramFdist,paramTh);
		return(numOfNodes);
}



















